#pragma once


namespace Math
{
	template <typename T>
	class Quaternion
	{
		T x;
		T y;
		T z;
		T w;

	public:
		Quaternion(void) {}
		~Quaternion(void) {}

		Quaternion(T _x, T _y, T _z, T _w)
		{
			x = _x;
			y = _y;
			z = _z;
			w = _w;
		}

		void make_identity()
		{
			x = T(0.0);
			y = T(0.0);
			z = T(0.0);
			w = T(1.0);
		}

		void normalize()
		{
			T length;
			T ilength;

			length = this->length();
		
			if ( length ) 
			{
				ilength = T(1 / length);
				x *= ilength;
				y *= ilength;
				z *= ilength;
				w *= ilength;
			}
		}

		void toMatrix( T *mtx )
		{
			// q3 implementation
			T	wx, wy, wz;
			T	xx, yy, yz;
			T	xy, xz, zz;
			T	x2, y2, z2;

			x2 = x + x;
			y2 = y + y;
			z2 = z + z;

			xx = x * x2;
			xy = x * y2;
			xz = x * z2;

			yy = y * y2;
			yz = y * z2;
			zz = z * z2;

			wx = w * x2;
			wy = w * y2;
			wz = w * z2;

			mtx[0] = T(1.0) - ( yy + zz );
			mtx[4] = xy - wz;
			mtx[8] = xz + wy;

			mtx[1] = xy + wz;
			mtx[5] = T(1.0) - ( xx + zz );
			mtx[9] = yz - wx;

			mtx[2] = xz - wy;
			mtx[6] = yz + wx;
			mtx[10] = T(1.0) - ( xx + yy );
		}


		template<typename T> friend Quaternion<T> operator * ( const Quaternion<T> & lhs, const Quaternion<T> & rhs )
		{	
			Quaternion<T> r;

			T A, B, C, D, E, F, G, H;

			A = (lhs.w + lhs.x) * (rhs.w + rhs.x);
			B = (lhs.z - lhs.y) * (rhs.y - rhs.z);
			C = (lhs.x - lhs.w) * (rhs.y + rhs.z);
			D = (lhs.y + lhs.z) * (rhs.x - rhs.w);
			E = (lhs.x + lhs.z) * (rhs.x + rhs.y);
			F = (lhs.x - lhs.z) * (rhs.x - rhs.y);
			G = (lhs.w + lhs.y) * (rhs.w - rhs.z);
			H = (lhs.w - lhs.y) * (rhs.w + rhs.z);

			r.w = B + (-E - F + G + H) * 0.5f;
			r.x = A - ( E + F + G + H) * 0.5f; 
			r.y =-C + ( E - F + G - H) * 0.5f;
			r.z =-D + ( E - F - G + H) * 0.5f;

			return r;
		}

		inline T length()
		{
			T length;
	
			length = a * a + b * b + c * c + d * d;
			return T(sqrt( length ));
		}

		void fromAxisAngle(T *axis, T theta)
		{
			// square norm of axis
			T sqnorm = axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2];

			if (sqnorm == T(0.0))
			{
				// axis too small.
				x = y = z = T(0.0);
				w = T(1.0);
			} 
			else 
			{
				theta *= T(0.5);
				T sin_theta = T(sin(theta));

				if ( sqnorm != T(1)) 
					sin_theta /= T(sqrt(sqnorm));
				x = sin_theta * axis[0];
				y = sin_theta * axis[1];
				z = sin_theta * axis[2];
				w = T(cos(theta));
			}
		}

	};

	typedef Quaternion<float> Quaternionf;
}